clear;
clc;
%读文件
ACCE_2x_1="D:\20240702_JT_0_0_1.txt";
ACCE_2x_2="D:\20240702_JT_0_0_2.txt";
ACCE_2x_3="D:\20240702_JT_0_0_3.txt";
ACCE_2x_4="D:\20240702_JT_0_0_4.txt";
ACCE_2x_5="D:\20240702_JT_0_0_5.txt";
ACCE_2x_6="D:\20240702_JT_0_0_6.txt";
Read_file2="D:\20003.ini";

%放到一个矩阵中，循环
acce=[ACCE_2x_1,ACCE_2x_2,ACCE_2x_3,ACCE_2x_4,ACCE_2x_5,ACCE_2x_6];
save ('acce.mat','acce');
figure;
for j=1:3
   subplot(2,2,j);
    hold on

for i=1:size(acce,2)
    data = load(acce(1,i));
    Ag(1:size(data,1),1:3,i) = data(:,1:3);

    
    FS=200;
    pi=3.1415926;
    ts = 1/FS;
    Params = load(Read_file2);
    %Ka0刻度系数，Ba0手动补偿零偏
    %标化，最后化成单位是°/h
    Ka0 = Params(11:13,:);
    Ba0 = Params(14,:);
    Ka2 = Params(15,:)* 1e-6;
    Ka(1,1)=1/(FS *Ka0(1,1));
    Ka(2,2)=1/(FS * Ka0(2,2));
    Ka(3,3)=1/(FS *Ka0(3,3));
    Ka(1,2)=Ka0(1,2)/60*pi/180 * Ka(2,2);
    Ka(1,3)= Ka0(1,3)/60*pi/180 *Ka(3,3);
    Ka(2,1)= Ka0(2,1)/60*pi/180 * Ka(1,1);
    Ka(2,3)=Ka0(2,3)/60*pi/180 *Ka(3,3);
    Ka(3,1)=Ka0(3,1)/60*pi/180 * Ka(1,1);
    Ka(3,2)=Ka0(3,2)/60*pi/180 * Ka(2,2);
    db = Ba0 * 0.01;
    A = Ag(1:size(data,1),1:3,i)*Ka';
    %计算在零输入状态下的输出值≈零偏
    AG = [(A(:,1)-db(1)*ts)*200, (A(:,2)-db(2)*ts)*200, (A(:,3)-db(3)*ts)*200];
    save('acc.mat','AG');
    %做10秒平滑
    AG=movmean(AG,20000);
    
    %算均值方差
    Ag_X=AG(:,1);
    Ag_X_mean=mean(Ag_X);
    Ag_X_std=std(Ag_X);
    
    Ag_Y=AG(:,2);
    Ag_Y_mean=mean(Ag_Y);
    Ag_Y_std=std(Ag_Y);
    
    Ag_Z=AG(:,3);
    Ag_Z_mean=mean(Ag_Z);
    Ag_Z_std=std(Ag_Z);

    %输出
 if j==1
    disp("10s平滑加表和陀螺平均值(m/s^2 m/s^2 m/s^2)");
    matrix = [Ag_X_mean,Ag_Y_mean,Ag_Z_mean];
    disp(matrix);
    disp("10s平滑加表和陀螺零偏稳定性(m/s^2 m/s^2 m/s^2)");
    matrix = [Ag_X_std,Ag_Y_std,Ag_Z_std];
    disp(matrix);
 end
    %画图
    switch j
      case 1
          plot(Ag_X);
          xlabel('x');
          ylabel('数值');
      case 2
          plot(Ag_Y); 
          xlabel('y');
          ylabel('数值');
      case 3
          plot(Ag_Z);
          xlabel('z');
          ylabel('数值');
end
  end
   end